package org.as4game.common.math
{
	import flash.geom.Matrix;
	import flash.geom.Matrix3D;
	import flash.geom.Orientation3D;
	import flash.geom.Vector3D;

	/**
	 * 数学辅助函数
	 */
	public class Math2
	{
		/**
		 * A reference to a Vector to be used as a temporary raw data container, to prevent object creation.
		 */
		public static const RAW_DATA_CONTAINER : Vector.<Number> = new Vector.<Number>(16);
		
		
		/**
		 * 向上对其到2的幂
		 */
		static public function up2Power( value:uint ):uint{
			var shift:int, v:int=value;
			while( v & 0xffffff00 ){
				shift += 8;
				v >>= 8;
			}
			while( v & 0xfc ){
				shift += 2;
				v >>= 2;
			}
			while( v ){
				shift ++;
				v >>= 1;
			}
			v = (shift ? (1 << (shift-1)) : 0);
			return (v == value? v: (v<<1) );
		}
		
		/**
		 * 从包围盒计算半径
		 * 
		 * @param aabb 包围盒
		 * @return 半径
		 */
		static public function boundingRadiusFromAABB(aabb:AxisAlignedBox):Number	{
			var max:Vector3 = aabb.mMaximum;
			var min:Vector3 = aabb.mMinimum;
			
			var max_inv:Vector3 = new Vector3;
			var min_inv:Vector3 = new Vector3;
			flip_v3(max_inv, max);
			flip_v3(min_inv, min);
			
			var ret:Vector3 = max.clone();
			makeCeil_v3_v3(ret, ret, max_inv);
			makeCeil_v3_v3(ret, ret, min);
			makeCeil_v3_v3(ret, ret, min_inv);
			
			return length_v3( ret );
		}
		
		/**
		 * 叉积, 6乘法
		 */
		static public function crossProduct_v3_v3(out:Vector3, lhs:Vector3, rhs:Vector3):void{
			var x:Number, y:Number, z:Number;
			x = lhs.y * rhs.z - lhs.z * rhs.y;
			y = lhs.z * rhs.x - lhs.x * rhs.z;
			z = lhs.x * rhs.y - lhs.y * rhs.x;
			out.x = x; out.y = y; out.z = z;
		}
		
		/**
		 * 相乘
		 */
		static public function multiply_v3_v3(out:Vector3, lhs:Vector3, rhs:Vector3):void{
			out.x = lhs.x * rhs.x;
			out.y = lhs.y * rhs.y;
			out.z = lhs.z * rhs.z;
		}
		static public function multiply_v3_n(out:Vector3, lhs:Vector3, n:Number):void{
			out.x = lhs.x * n;
			out.y = lhs.y * n;
			out.z = lhs.z * n;
		}
		static public function multiply_q_q(out:Quaternion, lhs:Quaternion, rhs:Quaternion):void{
			var w:Number, x:Number, y:Number, z:Number;
			w = lhs.w * rhs.w - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z,
			x = lhs.w * rhs.x + lhs.x * rhs.w + lhs.y * rhs.z - lhs.z * rhs.y,
			y = lhs.w * rhs.y + lhs.y * rhs.w + lhs.z * rhs.x - lhs.x * rhs.z,
			z = lhs.w * rhs.z + lhs.z * rhs.w + lhs.x * rhs.y - lhs.y * rhs.x
			out.w = w; out.x = x; out.y = y; out.z = z;
		}
		static public function multiply_q_v3(out:Vector3, lhs:Quaternion, rhs:Vector3):void{
//			// nVidia SDK implementation
//			Vector3 uv, uuv;
//			Vector3 qvec(x, y, z);
//			uv = qvec.crossProduct(v);
//			uuv = qvec.crossProduct(uv);
//			uv *= (2.0f * w);
//			uuv *= 2.0f;
//			
//			return v + uv + uuv;
			
			
			var uv:Vector3=new Vector3, uuv:Vector3=new Vector3;
			var qvec:Vector3 = new Vector3(lhs.x, lhs.y, lhs.y);
			
			crossProduct_v3_v3(uv, qvec, rhs);			// 6
			crossProduct_v3_v3(uuv, qvec, uv);			// 6
			multiply_v3_n(uv, uv, 2 * lhs.w);			// 3
			multiply_v3_n(uuv, uuv, 2);					// 3
			
			add_v3_v3(out, rhs, uv);
			add_v3_v3(out, out, uuv);
		}
		static public function multiply_q_n(out:Quaternion, lhs:Quaternion, n:Number):void{
			out.w = lhs.w * n;
			out.x = lhs.x * n;
			out.y = lhs.y * n;
			out.z = lhs.z * n;
		}
		
		/**
		 * out = maxof( lhs, rhs )
		 */
		static public function makeCeil_v3_v3(out:Vector3, lhs:Vector3, rhs:Vector3):void{
			out.x = ( rhs.x > lhs.x ? rhs.x: lhs.x );
			out.y = ( rhs.y > lhs.y ? rhs.y: lhs.y );
			out.z = ( rhs.z > lhs.z ? rhs.z: lhs.z );
		}
		
		/**
		 * out = minof( lhs, rhs )
		 */
		static public function makeFloor_v3_v3(out:Vector3, lhs:Vector3, rhs:Vector3):void{
			out.x = ( rhs.x < lhs.x ? rhs.x: lhs.x );
			out.y = ( rhs.y < lhs.y ? rhs.y: lhs.y );
			out.z = ( rhs.z < lhs.z ? rhs.z: lhs.z );
		}
		
		/**
		 * 取反
		 */
		static public function flip_v3(out:Vector3, v:Vector3):void{
			out.x = -v.x;
			out.y = -v.y;
			out.z = -v.z;
		}
		
		/**
		 * 取长度
		 */
		static public function length_v3(v:Vector3):Number{
			return Math.sqrt( v.x * v.x + v.y * v.y + v.z * v.z );
		}
		static public function length_q(q:Quaternion):Number{
			return Math.sqrt( q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
		}
		
		/**
		 * 相加
		 */
		static public function add_v3_v3(out:Vector3, lhs:Vector3, rhs:Vector3):void{
			out.x = lhs.x + rhs.x;
			out.y = lhs.y + rhs.y;
			out.z = lhs.z + rhs.z;
		}
		
		/**
		 * 单位化
		 */
		static public function identity_v3(v:Vector3D):void{
			v.w = v.x = v.y = v.z = 1;
		}
		
		/**
		 * Fills the 3d matrix object with values representing the transformation made by the given quaternion.
		 * 
		 * @param	quarternion	The quarterion object to convert.
		 */
		public static function quaternion2matrix(quarternion:Vector3D):Matrix3D
		{
			var x:Number = quarternion.x;
			var y:Number = quarternion.y;
			var z:Number = quarternion.z;
			var w:Number = quarternion.w;
			
			var xx:Number = x * x;
			var xy:Number = x * y;
			var xz:Number = x * z;
			var xw:Number = x * w;
			
			var yy:Number = y * y;
			var yz:Number = y * z;
			var yw:Number = y * w;
			
			var zz:Number = z * z;
			var zw:Number = z * w;
			
			var raw : Vector.<Number> = RAW_DATA_CONTAINER;
			raw[0] = 1 - 2 * (yy + zz); raw[1] = 2 * (xy + zw); raw[2] = 2 * (xz - yw); raw[4] = 2 * (xy - zw);
			raw[5] = 1 - 2 * (xx + zz); raw[6] = 2 * (yz + xw); raw[8] = 2 * (xz + yw); raw[9] = 2 * (yz - xw);
			raw[10] = 1 - 2 * (xx + yy);
			raw[3] = raw[7] = raw[11] = raw[12] = raw[13] = raw[14] = 0;
			raw[15] = 1;
			
			return new Matrix3D(raw);
		}
		
		/**
		 * 构造矩阵, 摘自 OGRE, Bone
		 */
		static public function makeTransform(position:Vector3D, scale:Vector3D, orientation:Vector3D):Matrix3D
		{
			// Ordering:
			//    1. Scale
			//    2. Rotate
			//    3. Translate
			
			// ToRotationMatrix
			var x:Number = orientation.x;
			var y:Number = orientation.y;
			var z:Number = orientation.z;
			var w:Number = orientation.w;
			
			var fTx:Number  = x+x;
			var fTy:Number  = y+y;
			var fTz:Number  = z+z;
			var fTwx:Number = fTx*w;
			var fTwy:Number = fTy*w;
			var fTwz:Number = fTz*w;
			var fTxx:Number = fTx*x;
			var fTxy:Number = fTy*x;
			var fTxz:Number = fTz*x;
			var fTyy:Number = fTy*y;
			var fTyz:Number = fTz*y;
			var fTzz:Number = fTz*z;
			
			var kRot00:Number = 1.0-(fTyy+fTzz);
			var kRot01:Number = fTxy-fTwz;
			var kRot02:Number = fTxz+fTwy;
			var kRot10:Number = fTxy+fTwz;
			var kRot11:Number = 1.0-(fTxx+fTzz);
			var kRot12:Number = fTyz-fTwx;
			var kRot20:Number = fTxz-fTwy;
			var kRot21:Number = fTyz+fTwx;
			var kRot22:Number = 1.0-(fTxx+fTyy);
			
			// Set up final matrix with scale, rotation and translation
			var m00:Number = scale.x * kRot00; var m01:Number = scale.y * kRot01; var m02:Number = scale.z * kRot02; var m03:Number = position.x;
			var m10:Number = scale.x * kRot10; var m11:Number = scale.y * kRot11; var m12:Number = scale.z * kRot12; var m13:Number = position.y;
			var m20:Number = scale.x * kRot20; var m21:Number = scale.y * kRot21; var m22:Number = scale.z * kRot22; var m23:Number = position.z;
			
			// No projection term
			var m30:Number = 0; var m31:Number = 0; var m32:Number = 0; var m33:Number = 1;
			
			//
			var arr:Vector.<Number> = new Vector.<Number>;
			arr.push( 
				m00, m01, m02, m03,
				m10, m11, m12, m13,
				m20, m21, m22, m23,
				m30, m31, m32, m33 );
			return new Matrix3D( arr );
		}

		/**
		 * 构造矩阵
		 */
		static public function matrix_build( quarternion:Vector3D, position:Vector3D, scale:Vector3D):Matrix3D{
			
			return makeTransform( position, scale, quarternion);
			
//			quaternion2matrix( quarternion, matrix);
//			matrix.appendTranslation( position.x, position.y, position.z );
//			matrix.appendScale( scale.x, scale.y, scale.z );
			
//			
//			var components:Vector.<Vector3D> = new Vector.<Vector3D>( 3 );
//			components[0] = position;
//			components[1] = quarternion;
//			components[2] = scale;
//			matrix.recompose(components, Orientation3D.EULER_ANGLES );
////			matrix.recompose( components, Orientation3D.QUATERNION );
		}
		
		public static function quaternion2euler_2(quarternion:Object):Vector3D{
			var q0:Number = quarternion.w;
			var q1:Number = quarternion.x;
			var q2:Number = quarternion.y;
			var q3:Number = quarternion.z;
			var v:Vector3D = new Vector3D;
			v.x = Math.atan2( 2 * (q2*q3 + q0*q1), (q0*q0 - q1*q1 - q2*q2 + q3*q3)); 
			v.y = Math.asin( -2 * (q1*q3 - q0*q2)); 
			v.z = Math.atan2( 2 * (q1*q2 + q0*q3), (q0*q0 + q1*q1 - q2*q2 - q3*q3));
			return v;
		}
		
		/**
		 * 根据2个向量求方向, v1->v2
		 * @return 然会 [ 旋转轴, 弧度 ]
		 * 
		 */
		public static function getRotate_v3_v3(v1:Vector3D, v2:Vector3D):Array{
			
			v1 = v1.clone();
			v2 = v2.clone();
			v1.normalize();
			v2.normalize();
			
			var v3:Vector3D = v1.crossProduct(v2);
			v3.normalize();
			var a:Number = Math.acos( v1.dotProduct(v2) );
			a = a * 180 / Math.PI;
			return [v3, a];
			
		}
		
		/**
		 * Returns a <code>Vector3D</code> object containing the euler angles represented by the given <code>Quaternion</code> object.
		 * 
		 * @param	m	The quaternion object to use in the calculation.
		 * @return		A 3d vector representing the euler angles extracted from the quaternion.
		 */
		public static function quaternion2euler(quarternion:Object):Vector3D
		{
			var result:Vector3D = new Vector3D();
			
			var test :Number = quarternion.x*quarternion.y + quarternion.z*quarternion.w;
			if (test > 0.499) { // singularity at north pole
				result.x = 2 * Math.atan2(quarternion.x,quarternion.w);
				result.y = Math.PI/2;
				result.z = 0;
				return result;
			}
			if (test < -0.499) { // singularity at south pole
				result.x = -2 * Math.atan2(quarternion.x,quarternion.w);
				result.y = - Math.PI/2;
				result.z = 0;
				return result;
			}
			
			var sqx	:Number = quarternion.x*quarternion.x;
			var sqy	:Number = quarternion.y*quarternion.y;
			var sqz	:Number = quarternion.z*quarternion.z;
			
			result.x = Math.atan2(2*quarternion.y*quarternion.w - 2*quarternion.x*quarternion.z , 1 - 2*sqy - 2*sqz);
			result.y = Math.asin(2*test);
			result.z = Math.atan2(2*quarternion.x*quarternion.w-2*quarternion.y*quarternion.z , 1 - 2*sqx - 2*sqz);
			
			return result;
		}
		
		/**
		 * 二分法查找
		 * @param arr 源数组, 必须是升序排序的
		 * @param key arr 中元素的字段名
		 * @param value 比较的值
		 * @return 返回 index, 使得 arr[index][key] 是不大于 value 的最大索引号, 如果失败, 返回-1
		 */
		static public function lower_bound(arr:Array, key:String, value:Object):int{
			var a:int = 0;
			var b:int = arr.length - 1;
			while( a <= b ){
				var c:int = (a + b ) / 2;
				var vc:Object = arr[c][key];
				if( vc < value){
					a = c+1;
				} 
				else if( vc > value){
					b = c-1;
				}
				else{
					return c;
				}
			}
			if( a >= arr.length ) return arr.length - 1;
			return ( arr[a][key] < value ? a: a-1);
		} 
	}
}